#include "opencv2/opencv.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"

#include <tf/transform_datatypes.h>

#include <iostream>
#include <stdio.h>
 
using namespace std;
using namespace cv;

// Scalar(b, g, r)

int main()
{
	// 设置窗口
	Mat img = Mat::zeros(Size(800, 800), CV_8UC3);
	
	img.setTo(255);              // 设置屏幕为白色
	
    //coordinate frame
    line(img, Point(50,  400), Point(750, 400), Scalar(0, 0, 0), 1); //x axis
	line(img, Point(200,  50), Point(200, 750), Scalar(0, 0, 0), 1); //y axis
	line(img, Point(100, 800), Point(750, 150), Scalar(0, 0, 0), 1); //line
    circle(img, Point(350, 400), 2, Scalar(255,0,0),-1);
    circle(img, Point(500, 400), 2, Scalar(255,0,0),-1);
    circle(img, Point(650, 250), 2, Scalar(255,0,0),-1);

    circle(img, Point(200, 550), 2, Scalar(255,0,0),-1);
    circle(img, Point(200, 700), 2, Scalar(255,0,0),-1);
    circle(img, Point(200, 400), 2, Scalar(0,0,255),-1);

    double r = 0.5; //m
    double c = 2 * CV_PI * r;
    double res = 0.05;
    size_t pts = c / res;
    double pt_rad =  2 * CV_PI / pts;
    double dest_rad = CV_PI / 4.0;
    size_t insert_pts = dest_rad / pt_rad;

    std::cout << "pt_rad: " << pt_rad << ", insert_pts: " << insert_pts << "\n";

    {
        Point O(200, 400);
        size_t pts0 = 30;
        double delta_th = CV_PI / 2.0 / pts0;
        tf::Quaternion q0 = tf::createQuaternionFromYaw(delta_th);
        tf::Transform trans0(q0, tf::Vector3(200, 400, 0));
        tf::Vector3 p1 = tf::Vector3(150*0.707, 150*0.707, 0);
        vector<Point> circle_quarter(pts0);
        tf::Vector3 lp = p1;
        size_t count = 0;
        for(auto& p : circle_quarter){
            q0 = tf::createQuaternionFromYaw(delta_th * count);
            trans0.setRotation(q0);
            tf::Vector3 tmp = trans0 * lp;
            p.x = tmp.x();
            p.y = tmp.y();
            // lp = tmp;
            ++count;
        }
        for(const auto& p : circle_quarter){
            std::cout << p << " ";
            circle(img, p, 1, Scalar(0,0,255),-1);
        }
        circle(img, circle_quarter.back(), 2, Scalar(0,255,0),-1);
        std::cout << "\n";
    }
    
    // Point sp(496, 390);
    // Point sp(500, 350);
    
    // insert_pts = 40;
    // Point sp(500-2*insert_pts+16, 400-4*insert_pts);
    // tf::Quaternion q = tf::createQuaternionFromYaw(pt_rad);
    // tf::Transform trans(q, tf::Vector3(sp.x, sp.y, 0));
    // vector<Point> vp;
    
    // vp.resize(insert_pts);
    // tf::Vector3 rot_p = tf::Vector3(0, 4*insert_pts, 0);
    // pt_rad = CV_PI / 4.0 / insert_pts;
    // // tf::Vector3 op = tf::Vector3(sp.x, sp.y, 0);
    // for(size_t i = 0; i< vp.size(); ++i){
    //     q = tf::createQuaternionFromYaw(-1.0 * pt_rad * i);
    //     // std::cout << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << " ";
    //     trans.setRotation(q);
    //     tf::Vector3 tmp = trans * rot_p;
    //     vp[i].x = tmp.x();
    //     vp[i].y = tmp.y();
    //     // std::cout << tmp.x() << " " << tmp.y() << " ";
    //     circle(img, vp[i], 1, Scalar(0, 0, 255),-1);
    //     // std::cout << vp[i] << "\n";
    // }
    // std::cout << "\n";

    // for(const auto& pt: vp){
    //     circle(img, pt, 2, Scalar(0,0,255),-1);
    //     std::cout << pt << " ";
    // }
    // std::cout << "\n";
    /*
	Point p1(100, 100);          // 点p1
	Point p2(758, 50);           // 点p2
 
	// 画直线函数
	line(img, p1, p2, Scalar(0, 0, 255), 2);   // 红色
	line(img, Point(300, 300), Point(758, 400), Scalar(0, 255, 255), 3);
 
	Point p(20, 20);//初始化点坐标为(20,20)
	circle(img, p, 1, Scalar(0, 255, 0), -1);  // 画半径为1的圆(画点）
 
	Point p4;
	p4.x = 300;
	p4.y = 300;
	circle(img, p4, 100, Scalar(120, 120, 120), -1);
 
	int thickness = 3;
	int lineType = 8;
	double angle = 30;  //椭圆旋转角度
	ellipse(img, Point(100, 100), Size(90, 60), angle, 0, 360, Scalar(255, 255, 0), thickness, lineType);
 
 
	// 画矩形
	Rect r(250, 250, 120, 200);
	rectangle(img, r, Scalar(0, 255, 255), 3);
    */
 
	imshow("draw_board", img);
	waitKey();
	return 0;
}